-------------------
-- Text Display
-------------------
function write (string, line)
  line = line or 1
  line = nxt.min(nxt.max(line, 1), 8)
  nxt.DisplayText(padded(string), 0, (line - 1) * 8)
end

function padded (string) 
  return deBoolean(string) .. "               "
end

cls = nxt.DisplayClear

abs = nxt.abs

function deBoolean (var)
  if isBoolean(var) then
    if var then
      return "true"
    else 
      return "false"
    end
  else
    return var
  end
end

function isBoolean (var) 
  return var == false or var == true
end

-------------------
-- Motors
-------------------

-- Motor Designations
A = 1
B = 2
C = 3

__defaultSpeed = 100

-- Motor Commands
function stop (motor)
  forward(motor, 0)
end

function float (motor)
  nxt.OutputSetSpeed(motor, 0, 0)
end

function forward (motor, speed) 
  speed = speed or __defaultSpeed
  nxt.OutputSetSpeed(motor, 32, speed)
  nxt.OutputSetRegulation(motor, 1, 1)
end

function backward (motor, speed)
  speed = speed or __defaultSpeed
  forward (motor, -speed)
end

function speedReading (motor)
  return nxt.OutputGetStatus(motor)
end

function rotationCounts (motor)
  _,counts = nxt.OutputGetStatus(motor)
  return counts
end

function resetRotationCount (motor)
  stop(motor)
  nxt.OutputResetTacho(motor, 1)
end

function resetAllMotors()
  resetRotationCount(A)
  resetRotationCount(B)
  resetRotationCount(C)
end

--------------
-- Buttons
--------------

-- Button Designations
ENTER = 8
ESCAPE = 1
LEFT = 4
RIGHT = 2

-- Button Command
isPressed = function (button) return nxt.ButtonRead() == button end

function __main()
cls()

forward (A)
forward (B)
repeat 
 if isTouched(1) then
 forward (A)
 forward (B, 50)
 elseif isTouched(2) then
 backward (A)
 backward (B, 20)
 else 
   forward(A)
   forward(B)
 end
 until rotationCounts(A) > 9000
 forward (A,80)
 forward (B,100)
repeat
 if isTouched(1) then
 forward (A)
 forward (B, 50)
 elseif isTouched(2) then
 backward (A)
 backward (B, 20)
 else 
   forward(A)
   forward(B)
 end
 until rotationCounts(A) > 20000
 forward (A)
 forward (B)
repeat
 if isTouched(1) then
 forward (A)
 forward (B, 50)
 elseif isTouched(2) then
 backward (A)
 backward (B, 20)
 else 
   forward(A)
   forward(B)
 end
 until rotationCounts(A) > 29000
stop (A)
stop (B)

end
repeat
cls()
resetAllMotors()
write("Press the ORANGE", 2)
write("button to START", 3)
write("your program", 4)
write("Press and hold", 6)
write("the GRAY button", 7)
write("when FINISHED", 8)
repeat until isPressed(ENTER)
__main()
until false
